//
// Created by lijiayi on 2020/2/10.
//
#include "ros/ros.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Odometry.h>
#include <iostream>

using namespace std;
using namespace ros;

geometry_msgs::Pose pose, drone, car;

void pos_rawCb(const geometry_msgs::PoseStamped &msg)
{
    pose = msg.pose;
}

void droneCb(const nav_msgs::Odometry &msg)
{
    drone = msg.pose.pose;
}

void carCb(const nav_msgs::Odometry &msg)
{
    car = msg.pose.pose;
}

int main(int argc, char *argv[]) {
    init(argc, argv, "compare_to_truth");
    NodeHandle nh;
    Publisher err_pub = nh.advertise<geometry_msgs::PoseStamped>("/error", 10);

    Subscriber pos_raw_sub = nh.subscribe("/cam/pose_raw", 10, pos_rawCb);
    Subscriber drone_sub = nh.subscribe("/mavros/local_position/odom", 10, droneCb);
    Subscriber car_sub = nh.subscribe("/car/ground_truth/state", 10, carCb);

    Rate rate(30);
    while(ros::ok())
    {
        ros::spinOnce();
        geometry_msgs::PoseStamped err;
        err.header.stamp = ros::Time::now();
        err.pose.position.x = drone.position.x - car.position.x ;//- pose.position.x;
        err.pose.position.y = drone.position.y - car.position.y ;//- pose.position.y;
        err.pose.position.z = drone.position.z - car.position.z ;//- pose.position.z;
        err_pub.publish(err);

        rate.sleep();

    }
    return 0;
}
